/*
 * TODO: Description
 *
 * Copyright (C) 2010 Johan Waldeback
 *  
 * This file is part of the OpenFMS project (http://www.openfms-project.org)
 * 
 * OpenFMS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Testing.h"
#include "OfmsMath.h"
using namespace OpenFms;
//extern float rho_half;

/**
 * Unit tests for toThrustScale(...)
 */
TESTSET(testsetToThrustScale)
{
	UNITTEST_FLOAT_EQUALS( 0.0f, toThrustScale(-1.2f) );
	UNITTEST_FLOAT_EQUALS( 0.0f, toThrustScale(-0.9f) );
	UNITTEST_FLOAT_EQUALS( 0.5f, toThrustScale( 0.0f) );
	UNITTEST_FLOAT_EQUALS( 1.0f, toThrustScale(+0.9f) );
	UNITTEST_FLOAT_EQUALS( 1.0f, toThrustScale(+1.2f) );
}

/**
 * Unit tests for clearForces(...)
 */
TESTSET( testsetClearForces )
{
	ModelSimulator_forces_t forces;
	forces.size = sizeof(ModelSimulator_forces_t);
	forces.force_x = 1.1f;
	forces.force_y = 1.2f;
	forces.force_z = 1.3f;
	forces.torque_x = 2.1f;
	forces.torque_y = 2.2f;
	forces.torque_z = 2.3f;

	clearForces( forces );

	UNITTEST_EQUALS( uint32_t, sizeof(ModelSimulator_forces_t), forces.size );
	UNITTEST_FLOAT_EQUALS( 0.0f, forces.force_x );
	UNITTEST_FLOAT_EQUALS( 0.0f, forces.force_y );
	UNITTEST_FLOAT_EQUALS( 0.0f, forces.force_z );
	UNITTEST_FLOAT_EQUALS( 0.0f, forces.torque_x );
	UNITTEST_FLOAT_EQUALS( 0.0f, forces.torque_y );
	UNITTEST_FLOAT_EQUALS( 0.0f, forces.torque_z );
}

/**
 * Unit tests for addForce(...)
 */
TESTSET( testsetAddForce )
{
	ModelSimulator_forces_t forces;

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 1,2,3 ), btVector3(0,0,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(1,2,3), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,0), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 1,0,0 ), btVector3(1,0,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(1,0,0), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,0), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 0,1,0 ), btVector3(1,0,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,1,0), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,1), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 0,0,1 ), btVector3(1,0,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,1), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,-1,0), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 1,0,0 ), btVector3(0,1,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(1,0,0), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,-1), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 0,1,0 ), btVector3(0,1,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,1,0), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,0), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 0,0,1 ), btVector3(0,1,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,1), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(1,0,0), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 1,0,0 ), btVector3(0,0,1) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(1,0,0), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,1,0), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 0,1,0 ), btVector3(0,0,1) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,1,0), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(-1,0,0), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 0,0,1 ), btVector3(0,0,1) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,1), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,0), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 0,1,0 ), btVector3(3,0,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,1,0), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,3), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )

	memset( &forces, 0, sizeof forces );
	addForce( forces, btVector3( 0,3,0 ), btVector3(1,0,0) );
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,3,0), btVector3(forces.force_x, forces.force_y, forces.force_z ) )
	UNITTEST_BTVECTOR3_EQUALS( btVector3(0,0,3), btVector3(forces.torque_x, forces.torque_y, forces.torque_z ) )
}

/**
 * Unit tests for forceOnSurface(...)
 */
/*TESTSET( testsetForceOnSurface )
{

}*/

// TODO: make RUN_TESTSET(...) below
// not to appear in the doxygen documentation
BEGIN_TESTING()
	RUN_TESTSET( testsetToThrustScale );
	RUN_TESTSET( testsetClearForces );
	RUN_TESTSET( testsetAddForce );
END_TESTING()

